#include "MyProject.h"
#include <stdlib.h>

float target = 0;
// 读取单个 ADC 通道数据的函数
/* USER CODE BEGIN 1 */
uint32_t analogRead(uint32_t Channel)
{
	ADC_ChannelConfTypeDef sConfig = {0};
  sConfig.Channel = Channel;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES_5;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
	HAL_ADC_Start(&hadc1);
	HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
  uint16_t value = 0;
  if(HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc1), HAL_ADC_STATE_EOC_REG)) 
  {
    value = HAL_ADC_GetValue(&hadc1);
    HAL_ADC_Stop(&hadc1);
    return value;
  }
  HAL_ADC_Stop(&hadc1);
	return 0;
}

// function reading an ADC value and returning the read voltage
float _readADCVoltageInline(uint32_t channel)
{
  unsigned short raw_adc = analogRead(channel);
  return (float)raw_adc * 3.3 / 4096;
}

/* USER CODE BEGIN 2 */
// 控制 LED 开
void LED_On(void)
{
  HAL_GPIO_WritePin(LED_GPIO_PORT, LED_PIN, GPIO_PIN_SET); // 设置 PA1 为高电平
}

// 控制 LED 关
void LED_Off(void)
{
  HAL_GPIO_WritePin(LED_GPIO_PORT, LED_PIN, GPIO_PIN_RESET); // 设置 PA1 为高电平
}

void led_tick()
{
  static bool tick_flag = true;
  tick_flag = !tick_flag;
  if (tick_flag == true)
  {
    LED_On();
  }
  else
  {
    LED_Off();
  }
}

void GPIO_Key_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    // 1. 启用 GPIOA 时钟
    __HAL_RCC_GPIOA_CLK_ENABLE();
    // 2. 配置 PA7 为输入模式，使用内部上拉电阻
    GPIO_InitStruct.Pin = KEY_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;   // 下降沿触发中断（按键按下时）
    GPIO_InitStruct.Pull = GPIO_PULLUP;            // 上拉电阻
    HAL_GPIO_Init(KEY_GPIO_Port, &GPIO_InitStruct);
    // 3. 启用并配置 EXTI 线的中断优先级
    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 2, 0); // 配置优先级，EXTI 线 5-9
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);         // 启用 EXTI9_5 中断
}

void EXTI9_5_IRQHandler(void)
{
  // 检查 PA7 的中断标志
  if (__HAL_GPIO_EXTI_GET_IT(KEY_Pin) != RESET)
  {
    __HAL_GPIO_EXTI_CLEAR_IT(KEY_Pin); // 清除中断标志
    // 在这里处理按键按下的事件
    // HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13); // 例如，切换 LED 状态
    led_tick();

  }
}

void LED_Breathing_Light()
{
  int value = 0;
  while (value < 500)
  {
    value++;
    __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, value);
    HAL_Delay(1);
  }
  while (value)
  {
    value--;
    __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, value);
    HAL_Delay(1);
  }
}
/******************************************************************************/
unsigned char USART_RX_BUF[USART_REC_LEN]; // 接收缓冲，usart.h中定义长度
// 接收状态
// bit15  接收完成标志
// bit14  接收到0x0D
// bit13~0  接收的字节数
unsigned short USART_RX_STA = 0; // 接收状态标志

int _write(int file, char *data, int len)
{
  HAL_UART_Transmit(&huart2, (uint8_t *)data, len, HAL_MAX_DELAY);
  return len;
}

void USART2_IRQHandler(void)
{
  /* USER CODE BEGIN 1 */
  uint8_t rx_data = 0; // USART1接收到的数据
  if (__HAL_UART_GET_FLAG(&huart2, UART_FLAG_RXNE))
  {
    HAL_UART_Receive(&huart2, &rx_data, 1, HAL_MAX_DELAY);
    // 处理 USART1 接收到的数据，例如存入缓冲区
    // ProcessReceivedData(rx1_data);
    HAL_UART_Transmit(&huart3, &rx_data, 1, HAL_MAX_DELAY);
    if ((USART_RX_STA & 0x8000) == 0) // 接收未完成
    {
      if (USART_RX_STA & 0x4000) // 接收到了0x0d
      {
        if (rx_data != 0x0a)
          USART_RX_STA = 0; // 接收错误，重新开始
        else
        {
          USART_RX_STA |= 0x8000;                     // 接收完成
          USART_RX_BUF[USART_RX_STA & 0X3FFF] = '\0'; // 最后一个字节放'0’，方便判断
        }
      }
      else // 还没收到0x0D
      {
        if (rx_data == 0x0d)
          USART_RX_STA |= 0x4000;
        else
        {
          USART_RX_BUF[USART_RX_STA & 0X3FFF] = rx_data;
          USART_RX_STA++;
          if (USART_RX_STA > (USART_REC_LEN - 1))
            USART_RX_STA = 0; // 接收错误，重新开始
        }
      }
    }
  }
  HAL_UART_IRQHandler(&huart2);
  led_tick();
  // HAL_Delay(500);
}

void USART3_IRQHandler(void)
{
  /* USER CODE BEGIN 1 */
  uint8_t rx_data = 0; // USART1接收到的数据
  if (__HAL_UART_GET_FLAG(&huart3, UART_FLAG_RXNE))
  {
    HAL_UART_Receive(&huart3, &rx_data, 1, HAL_MAX_DELAY);
    HAL_UART_Transmit(&huart2, &rx_data, 1, HAL_MAX_DELAY);
  }
  HAL_UART_IRQHandler(&huart3);
  led_tick();
  // HAL_Delay(500);
}

/* USER CODE END 1 */

/******************************************************************************/
// 通信协议只有简单的设置速度，不能与官方上位机对接
void commander_run(void)
{
  PhaseCurrent_s current;
  if ((USART_RX_STA & 0x8000) != 0)
  {
    switch (USART_RX_BUF[0])
    {
    case 'A': // 打印当前机械角度
      torque_controller = Type_voltage; // Type_dc_current;//  Type_foc_current;  //Type_voltage;
	    controller = Type_angle;		  // Type_torque;  //Type_velocity;  //Type_angle;
      target = 0;
      break;
    case 'V': // 打印当前机械角度
      torque_controller = Type_voltage; // Type_dc_current;//  Type_foc_current;  //Type_voltage;
	    controller = Type_velocity;		      // Type_torque;  //Type_velocity;  //Type_angle;
      target = 6.28;
      break;
    case 'B': // 电流
      printff("current.a=", _readADCVoltageInline(ADC_CHANNEL_1));
      printff("current.b=", _readADCVoltageInline(ADC_CHANNEL_5));
      break;
    case 'H':
      printf("Hello World!\r\n");
      break;
    case 'T': // T6.28
      target = atof((const char *)(USART_RX_BUF + 1));
      printff("T=", target);
      break;
    case 'P': // P,设置速度模式的P参数,例如设置P=0.8，发送指令P0.8
      PID_velocity.P = atof((const char *)(USART_RX_BUF + 1));
      printff("P=", PID_velocity.P);
      break;
    case 'I': // I,设置速度模式的I参数,例如设置I=0.8，发送指令I0.8
      PID_velocity.I = atof((const char *)(USART_RX_BUF + 1));
      printff("I=", PID_velocity.I);
      break;
    }
    USART_RX_STA = 0;
  }
}

void enable_module()
{
  GPIO_Key_Init();
  __HAL_UART_ENABLE_IT(&huart2, UART_IT_RXNE);
  __HAL_UART_ENABLE_IT(&huart3, UART_IT_RXNE);

  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
}

void PWM_TEST()
{
  __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, 700);
  __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, 700);
  __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, 700);
}

/* USER CODE BEGIN 2 */
void i2c_bus_scan()
{
  /* USER CODE BEGIN 2 */
  uint8_t device_addr[128] = {0};
  uint8_t cnt = 0;
  printf("Scanning I2C bus:\r\n");
  HAL_StatusTypeDef result;
  uint8_t i;
  for (i = 1; i < 128; i++)
  {
    /*
    * the HAL wants a left aligned i2c address
    * &hi2c1 is the handle
    * (uint16_t)(i<<1) is the i2c address left aligned
    * retries 2
    * timeout 2
        i2c address:（0x78 = 0x3c << 1）
    */
    result = HAL_I2C_IsDeviceReady(&hi2c1, (uint16_t)(i << 1), 2, 2);
    if (result != HAL_OK) // HAL_ERROR or HAL_BUSY or HAL_TIMEOUT
    {
      printf("."); // No ACK received at that address
    }
    if (result == HAL_OK)
    {
      printf("0x%X", i); // Received an ACK at that address
      device_addr[cnt++] = i;
    }
    HAL_Delay(10);
  }
  printf("\r\n");
  if(cnt > 0) {
    printf("Find device addr: ");
    for(int i = 0; i < cnt; i++)
      printf("0x%02x " , device_addr[i]);
    printf("\r\n");
  } else {
    printf("Not Find device any device ....... then reset systrem \n");
    HAL_NVIC_SystemReset();
  }
  HAL_Delay(2000);
}


void printff(const char *name, float data) {
  uint16_t u16Value = (uint16_t)data;
  uint16_t fValue = (data - u16Value)*100000;
  printf("%s=%d.%d\r\n",name, u16Value,fValue);
}

void printfff(float data) {
  uint16_t u16Value = (uint16_t)data;
  uint16_t fValue = (data - u16Value)*100000;
  printf("%d.%d", u16Value,fValue);
}

void Sensor_check() {
  GPIO_Key_Init();
  /* USER CODE BEGIN 2 */
  i2c_bus_scan();
  /* USER CODE END 2 */
  //MX_TIM2_Init();
	//HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
  // PWM_TEST();
  /* Infinite loop */
}

// 0xFFFFFF到0循环计数
void systick_CountMode(void)//滴答时钟为HCLK/8=72M/8=9Mhz
{
  SysTick->LOAD = 0xFFFFFF-1;      //set reload register
  SysTick->VAL  = 0;
  SysTick->CTRL = SysTick_CTRL_ENABLE_Msk; //Enable SysTick Timer
}

uint32_t micros(void) {
  return SysTick->VAL;
}


#ifdef DENGFOC_CODE
void Foc_init(float _target) {
  int Sensor_DIR=-1;    //传感器方向
  int Motor_PP=7;    //电机极对数
  target = _target;
  /* USER CODE END 2 */
  DFOC_Vbus(7.4);   //设定驱动器供电电压
  DFOC_M0_alignSensor(Motor_PP,Sensor_DIR);
}

void Foc_run(float _target) {
    runFOC();
    //输出角度值
    // float Kp=0.133;
    // float Sensor_Angle=DFOC_M0_Angle();
    M0_setTorque(_target,S0_electricalAngle());   //电压力矩
    // DFOC_M0_SET_ANGLE_PID(1,0,0,100000,30);
    // DFOC_M0_SET_VEL_PID(0.02,1,0,100000,0.5);
    // DFOC_M0_SET_CURRENT_PID(5,200,0,100000);
    // DFOC_M0_set_Velocity_Angle(_target);

    //serialReceiveUserCommand();
    //HAL_Delay(1000);
    //printff("angle  = \n",_readADCVoltageInline(ADC_CHANNEL_5));
}
#endif
#ifdef SIMPLEFOC_CODE
void Foc_init(float _targe) {

	MagneticSensor_Init();										   // 初始化编码器参数 和 I2C或者SPI
	InlineCurrentSense(0.03, 50, ADC_CHANNEL_1, ADC_CHANNEL_5, NOT_SET); // SimpleMotor//采样电阻阻值，运放倍数，A相，B相，C相
	InlineCurrentSense_Init();									   // ADC初始化和偏置电压校准
	LPF_init();													   // LPF参数初始化
	PID_init();													   // PID参数初始化

	voltage_power_supply = 7.4;		  // V 电源电压
	pole_pairs = 7;					  // 电机极对数，按照实际设置，虽然可以上电检测但有失败的概率
	voltage_sensor_align = 2;		  // V alignSensor() use it，航模电机设置的值小一点比如0.5-1，云台电机设置的大一点比如2-3
	voltage_limit = 7;				  // V，主要为限制电机最大电流，最大值需小于12/1.732=6.9
	velocity_limit = 100;			  // rad/s 角度模式时限制最大转速，力矩模式和速度模式不起作用
	current_limit = 2;				  // A，foc_current和dc_current模式限制电流，不能为0。速度模式和位置模式起作用
	torque_controller = Type_voltage; // Type_dc_current;//  Type_foc_current;  //Type_voltage;
	controller = Type_angle;		  // Type_torque;  //Type_velocity;  //Type_angle;
	PID_current_d.P = 0.6;			  // 电流环PI参数，可以进入 PID_init() 函数中修改其它参数
	PID_current_d.I = 0;			  // 电流环I参数不太好调试，设置为0只用P参数也可以
	PID_current_q.P = 0.6;
	PID_current_q.I = 0;
	PID_velocity.P = 0.1; // 0.5, 速度环PI参数，只用P参数方便快速调试
	PID_velocity.I = 1;
	P_angle.P = 20;				   // 位置环参数，只需P参数
	PID_velocity.output_ramp = 50; // 速度爬升斜率，如果不需要可以设置为0
	LPF_velocity.Tf = 0.0001;	   // Tf设置小一点，配合爬升斜率设置，速度切换更平稳；如果没有爬升模式的斜率限制，Tf太小电机容易抖动。

	target = _targe;

	Motor_init();
	Motor_initFOC(0, UNKNOWN); //(2.2,CW);(0,UNKNOWN);  //如果填入零点偏移角度和方向，将跳过上电检测。电机极对数要设置正确。
	printf("Motor ready.\r\n");
	/******************************************************************************/
}

void Foc_run(float _target) {
  	move(_target);
		loopFOC();
		commander_run();
}
#endif


// 定时器中断回调
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    if (htim->Instance == TIM2) {
        // 1ms 定时中断逻辑
        led_tick();
    }
}